/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : pipo.cpp
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <avr/eeprom.h>

#include "pipo.h"
#include "eeprom.h"
#include "sensors.h"
#include "i2cmaster.h"
#include "imu.h"
#include "mixer.h"
#include "pwm.h"
#include "uart.h"
//#include "eepromData.h"

//#define _SIMULATOR

/*************************************************************************
 Global Variables
*************************************************************************/

volatile uint16_t g25msec; // increments every 25ms

volatile uint8_t flashRedLed;	// set this = the # of flashes @ 5fps you want from teh red led
volatile uint8_t flashGreenLed;	// set this = the # of flashes @ 5fps you want from teh green led

static uint16_t lastTCNT1;
uint16_t deltaT_int;

uint8_t operationsLed = 0;

int16_t	attitudeCMD[3];  // Pitch,Roll
uint8_t magnetometer = 0;
uint16_t maxPWMPeriod = PWM_FRAME_TICKS;

/*************************************************************************
 delay
*************************************************************************/
void delay(uint16_t msec)
{
    uint16_t tgt = g25msec + msec/25;
    while(tgt != g25msec);
}

void shortDelay(uint16_t tTicks)
{
    uint16_t dt,st;

    cli();
    st = TCNT1;
    sei();

    do {
        cli();
        dt = TCNT1;
        sei();
        dt -= st;
    } while(dt < tTicks);
}


//syncs main loop and calculates deltaT_int
void mainLoopSync()
{
    uint8_t c = 2;
    do {		//calculate real deltaT between measurements
        if(c) c--;
        cli();
        deltaT_int = TCNT1;
        sei();
        deltaT_int -= lastTCNT1;	// deltaT_int = curr_time - last_time
    } while(deltaT_int < maxPWMPeriod);  //if frame is longer than specified time then just go through

    if(c) LED_RED_ON;

    lastTCNT1 += deltaT_int;  // last_time = last_time + deltaT_int
}

void calcMaxPWMPeriod()
{
    maxPWMPeriod = 0;
    for(uint8_t i=0; i<NUM_CHNOUT; i++)
    {
        if(eeData.limitData[i].period>maxPWMPeriod) maxPWMPeriod = eeData.limitData[i].period;
    }

    if(maxPWMPeriod>30000) maxPWMPeriod = 30000;
    maxPWMPeriod <<= 1; //multiply by 2 to get tic num per uS
}

/*************************************************************************
 25msec interrupt
*************************************************************************/
ISR(TIMER1_COMPB_vect, ISR_NOBLOCK)  // fires on TCNT1 == OCR1B - 25ms counter
{
    g25msec++;

    if(operationsLed)
    {
        if(!flashRedLed && !flashGreenLed)
        {
            if(g25msec & 0x1F)
                LED_GREEN_OFF;
            else
                LED_GREEN_ON;
        }
    }

    if(!(g25msec & 0x07)) // every 8 x 25ms (200ms)
    {
        if(flashGreenLed)
        {
            if(LED_GREEN_IS_ON) flashGreenLed--;
            LED_GREEN_FLIP;
        }

        if(flashRedLed)
        {
            if(LED_RED_IS_ON) flashRedLed--;
            LED_RED_FLIP;
        }
    }

    OCR1B += G25MSEC_SPAN; // next 25ms counter.
}

void setupPins()
{
    //init pins
    DDRB  = PORT_B_SETUP;    PORTB = PORT_B_INIT;
    DDRC  = PORT_C_SETUP;    PORTC = PORT_C_INIT;
    DDRD  = PORT_D_SETUP;    PORTD = PORT_D_INIT;

    //set up input pin change interrupt for inputs
    PCMSK0 |= PCMSK0_BITS;
    PCMSK1 |= PCMSK1_BITS;
    PCMSK2 |= PCMSK2_BITS;

    PCICR  |= PCICR_BITS; //enable pin change interrupts (globals are disabled at the moment)
}

/*************************************************************************
 main
*************************************************************************/
int main(void)
{
    // disable watchdog after the soft reset
    MCUSR = 0;
    wdt_disable();

    readEEPROM();
    calcMaxPWMPeriod();

    setupPins();

    //set up timers
    TCCR1A = 0;				//set up 16bit timer,
    TCCR1B = (1<<CS11);  	// 8 prescaler - gives us 16,000/8 => 2000 counts pes msec

    LED_RED_ON; LED_GREEN_ON; // Green on = device initiating

    //Initialise the UART
    uart_init(57600);

    // initialize I2C library
    i2c_init();

    operationsLed = 0;
    OCR1B = TCNT1 + 10;
    TIMSK1 |= (1<<OCIE1B); // enable 25ms counter interrupt

    sei();

    delay(STARTUP_DELAY); // allow rx to come online - green LED only ON
    // FlySky RX takes ~350ms to start up

    LED_GREEN_ON;LED_RED_OFF;

    // set up L3G4200D
    flashGreenLed = l3g4200d_init() ? 0 : 0xFF;  // flash green led 255 times to show error

    // set up ADXL345
    flashRedLed = adxl_init() ? 0 : 0xFF;   // flash red led 255 times to show error

    magnetometer = HMC5883L_init();

    delay(250);

    calibrateGyro(); //flashes red led



    LED_RED_OFF; LED_GREEN_OFF; // leds off = ready to run!
    operationsLed = 1;
    flashGreenLed = flashRedLed = 2; // flash both to signify operations start

    cli();
    lastTCNT1 = TCNT1;
    sei();

    //main loop
    while (1)
    {
        //loop without PWM is 2200us
        // calculate IMU and populate attitude vars
        if(magnetometer)
            updateAHRS();
        else
            updateIMU();

        getAttitude();

        //READ sensor data values
        populateSensorData();

        mixer();

        //syncs main loop and calculates deltaT_int
        mainLoopSync();


/*************************************************************************************************/
//  PWM ROUTINES
/*************************************************************************************************/
        outputPWM();

    }
}
